/*
Autor: Alexander Savochkin, 2010
New BSD License
*/

#include "PIDRegulator.h"   
#include "FactoriesKeeper.h"
#include "Exception.h"

namespace SRCLF 
{
namespace Control 
{

	static bool registered = FactoriesKeeper::getInstance().registerRegulatorFactory("PID", 
				static_cast<RegulatorFactory*>(new DefaultRegulatorFactory<PIDRegulator>()));

	void PIDRegulator::init(double current_input, double current_output, const ptime& current_time)
	{
		last_update = current_time;
		regulator_value = current_output;
		error_integral = 0.0;
		last_mean_derivative = 0;
		last_err = 0;
	}

	void PIDRegulator::update(double input_val, const ptime& new_time)
	{
		double dt = 1.e-3 * (new_time - last_update).total_milliseconds();
		double current_err = input_val - desired;		
		double de_by_dt = (current_err - last_err)/dt;
		double mean_de_by_dt = de_by_dt * dt / exp_moving_average_period + 
			(1.0 - dt/exp_moving_average_period) * last_mean_derivative;
		error_integral += dt * current_err;	//TODO: Increase accuracy by using 
		regulator_value = -P*current_err - I * error_integral - D * mean_de_by_dt;
		last_update = new_time;
		last_mean_derivative = mean_de_by_dt;
		last_err = current_err;
	}

	double PIDRegulator::getRegulatorValue()
	{
		return regulator_value;
	}
	 
	void PIDRegulator::adminCommand(const string& command, string& result)
	{
	}

	void PIDRegulator::controlCommand(const string& command, string& result)
	{
		//There are no any recognized commands
	}

	void PIDRegulator::infoCommand(const string& command, string& result)
	{
		//There are 3 recognozed commands: get_coefficients(P,I,D) and get_diff_smooth_time(T) and get_history

		string cmd;
		if(cmd == "get_coefficients")
		{

		}
		else if(cmd == "get_diff_smooth_time")
		{

		}
		else if(cmd == "get_history")
		{

		}
		else
		{
			
		}
	}

} //namespace Control
} //namespace SRCLF
